
package graphics3d;


import static java.lang.Math.atan;
import static java.lang.Math.cos;
import static java.lang.Math.sin;


public class Motion {
    
    protected final Matrix object;
    protected final int pivot_point, pivot_point_2;
    protected Matrix conversions;
    
    Motion(Matrix object) {
        this(object, 0);
    }
    Motion(Matrix object, int pivot_point, int pivot_point_2) {
        this.object = object;
        this.pivot_point = pivot_point;
        this.pivot_point_2 = pivot_point_2;
        this.initConversions();
    }
    Motion(Matrix object, int pivot_point) {
        this(object, pivot_point, 0);
    }
    
    private void initConversions() {
        double[] arr = new double[]{1, 0, 0, 0, 
                                    0, 1, 0, 0,
                                    0, 0, 1, 0, 
                                    0, 0, 0, 1};
        conversions = new Matrix(4, arr);
    }
    
    public void reset() {
        this.initConversions();
    }
    
    public Motion transfer(double a, double b, double c) {
        double[] arr = new double[]{1, 0, 0, a,
                                    0, 1, 0, b,
                                    0, 0, 1, c,
                                    0, 0, 0, 1};
        Matrix transfer = new Matrix(4, arr);
        conversions = transfer.multiply(conversions);
        
        return this;
    }
    public Motion rotateZ(double angle) {
        angle *= atan(1)/45;
        double[] arr = new double[]{cos(angle), -sin(angle), 0, 0,
                                    sin(angle),  cos(angle), 0, 0, 
                                             0,           0, 1, 0,
                                             0,           0, 0, 1};
        Matrix rotate = new Matrix(4, arr);
        conversions = rotate.multiply(conversions);
        
        return this;
    }
    public Motion rotateX(double angle) {
        angle *= atan(1)/45;
        double[] arr = new double[]{1,          0,           0, 0,
                                    0, cos(angle), -sin(angle), 0, 
                                    0, sin(angle),  cos(angle), 0,
                                    0,          0,           0, 1};
        Matrix rotate = new Matrix(4, arr);
        conversions = rotate.multiply(conversions);
        
        return this;
    }
    public Motion rotateY(double angle) {
        angle *= atan(1)/45;
        double[] arr = new double[]{ cos(angle), 0, sin(angle), 0,
                                              0, 1,          0, 0,
                                    -sin(angle), 0, cos(angle), 0, 
                                              0, 0,          0, 1};
        Matrix rotate = new Matrix(4, arr);
        conversions = rotate.multiply(conversions);
        
        return this;
    }
    public Motion scale(double kx, double ky, double kz) {
        double[] arr = new double[]{kx,  0,  0, 0,
                                    0,  ky,  0, 0,
                                    0,   0, kz, 0,
                                    0,   0,  0, 1};
        Matrix scale = new Matrix(4, arr);
        conversions = scale.multiply(conversions);
        
        return this;
    }
    public Motion reflectXY() {
        double[] arr = new double[]{1, 0,  0, 0, 
                                    0, 1,  0, 0, 
                                    0, 0, -1, 0,
                                    0, 0,  0, 1};
        Matrix reflect = new Matrix(4, arr);
        conversions = reflect.multiply(conversions);
        
        return this;
    }
    public Motion reflectYZ() {
        double[] arr = new double[]{-1, 0, 0, 0, 
                                     0, 1, 0, 0, 
                                     0, 0, 1, 0,
                                     0, 0, 0, 1};
        Matrix reflect = new Matrix(4, arr);
        conversions = reflect.multiply(conversions);
        
        return this;
    }
    public Motion reflectZX() {
        double[] arr = new double[]{1,  0, 0, 0, 
                                    0, -1, 0, 0, 
                                    0,  0, 1, 0,
                                    0,  0, 0, 1};
        Matrix reflect = new Matrix(4, arr);
        conversions = reflect.multiply(conversions);
        
        return this;
    }
    public Motion reflectX() {
        double[] arr = new double[]{1,  0,  0, 0, 
                                    0, -1,  0, 0, 
                                    0,  0, -1, 0,
                                    0,  0,  0, 1};
        Matrix reflect = new Matrix(4, arr);
        conversions = reflect.multiply(conversions);
        
        return this;
    }
    public Motion reflectY() {
        double[] arr = new double[]{-1, 0,  0, 0, 
                                     0, 1,  0, 0, 
                                     0, 0, -1, 0,
                                     0, 0,  0, 1};
        Matrix reflect = new Matrix(4, arr);
        conversions = reflect.multiply(conversions);
        
        return this;
    }
    public Motion reflectZ() {
        double[] arr = new double[]{-1,  0, 0, 0, 
                                     0, -1, 0, 0, 
                                     0,  0, 1, 0,
                                     0,  0, 0, 1};
        Matrix reflect = new Matrix(4, arr);
        conversions = reflect.multiply(conversions);
        
        return this;
    }
    public Motion reflectO() {
        double[] arr = new double[]{-1,  0,  0, 0, 
                                     0, -1,  0, 0, 
                                     0,  0, -1, 0,
                                     0,  0,  0, 1};
        Matrix reflect = new Matrix(4, arr);
        conversions = reflect.multiply(conversions);
        
        return this;
    }
    
    public Motion rotateX(double x0, double y0, double z0, double angle) {
        return transfer(-x0, -y0, -z0).rotateX(angle).transfer(x0, y0, z0);
    }
    public Motion rotateY(double x0, double y0, double z0, double angle) {
        return transfer(-x0, -y0, -z0).rotateY(angle).transfer(x0, y0, z0);
    }
    public Motion rotateZ(double x0, double y0, double z0, double angle) {
        return transfer(-x0, -y0, -z0).rotateZ(angle).transfer(x0, y0, z0);
    }
    
    public Motion rotatePPX(double angle) {
        return rotateX(conversions.multiply(object).cell(0, pivot_point), conversions.multiply(object).cell(1, pivot_point), conversions.multiply(object).cell(2, pivot_point), angle);
    }
    public Motion rotatePPY(double angle) {
        return rotateY(conversions.multiply(object).cell(0, pivot_point), conversions.multiply(object).cell(1, pivot_point), conversions.multiply(object).cell(2, pivot_point), angle);
    }
    public Motion rotatePPZ(double angle) {
        return rotateZ(conversions.multiply(object).cell(0, pivot_point), conversions.multiply(object).cell(1, pivot_point), conversions.multiply(object).cell(2, pivot_point), angle);
    }
    
    public Motion RotatePPLine(double angle) {
        double[] arr = new double[]{1, 0, 0, 0,
                                    0, 1, 0, 0,
                                    0, 0, 1, 0,
                                    0, 0, 0, 1};
        Matrix currentConversions = new Matrix(4, arr);
        Matrix currentObject = conversions.multiply(object);
        double ax, ay, az;
        ax = currentObject.cell(0, pivot_point);
        ay = currentObject.cell(1, pivot_point);
        az = currentObject.cell(2, pivot_point);
//        this.transfer(-ax, -ay, -az);
        arr = new double[]{1, 0, 0, -ax,
                           0, 1, 0, -ay,
                           0, 0, 1, -az,
                           0, 0, 0,   1};
        Matrix transfer = new Matrix(4, arr);
        currentConversions = transfer.multiply(currentConversions);
        
        double cosAlpha, sinAlpha, cosBeta, sinBeta;
        double x, y, z;
        currentObject = currentConversions.multiply(conversions.multiply(object));
        x = currentObject.cell(0, pivot_point_2);
        y = currentObject.cell(1, pivot_point_2);
        z = currentObject.cell(2, pivot_point_2);
        double sqrt1 = Math.sqrt(x*x + y*y);
        double sqrt2 = Math.sqrt(x*x + y*y + z*z);
        sinAlpha = y/sqrt1;
        cosAlpha = x/sqrt1;
        sinBeta = z/sqrt2;
        cosBeta = sqrt1/sqrt2;
        
        arr = new double[]{ cosAlpha, sinAlpha, 0, 0,
                           -sinAlpha, cosAlpha, 0, 0,
                                   0,        0, 1, 0,
                                   0,        0, 0, 1};
        Matrix conversion = new Matrix(4, arr);
        currentConversions = conversion.multiply(currentConversions);
        
        arr = new double[]{cosBeta, 0, -sinBeta, 0,
                                 0, 1,        0, 0,
                           sinBeta, 0,  cosBeta, 0,
                                 0, 0,        0, 1};
        conversion = new Matrix(4, arr);
        currentConversions = conversion.multiply(currentConversions);
        
//        conversions = currentConversions.multiply(conversions);
//        this.rotateX(angle);
//        this.reflectX();
        angle *= atan(1)/45;
        arr = new double[]{1,          0,           0, 0,
                           0, cos(angle), -sin(angle), 0, 
                           0, sin(angle),  cos(angle), 0,
                           0,          0,           0, 1};
        Matrix rotate = new Matrix(4, arr);
        currentConversions = rotate.multiply(currentConversions);
        
        arr = new double[]{cosAlpha, -sinAlpha, 0, 0,
                           sinAlpha,  cosAlpha, 0, 0,
                                  0,         0, 1, 0,
                                  0,         0, 0, 1};
        conversion = new Matrix(4, arr);
        currentConversions = conversion.multiply(currentConversions);
        
        arr = new double[]{ cosBeta, 0, sinBeta, 0,
                                  0, 1,       0, 0,
                           -sinBeta, 0, cosBeta, 0,
                                  0, 0,       0, 1};
        conversion = new Matrix(4, arr);
        currentConversions = conversion.multiply(currentConversions);
        
//        this.transfer(ax, ay, az);
        arr = new double[]{1, 0, 0, ax,
                           0, 1, 0, ay,
                           0, 0, 1, az,
                           0, 0, 0,  1};
        transfer = new Matrix(4, arr);
        currentConversions = transfer.multiply(currentConversions);
        
        conversions = currentConversions.multiply(conversions);
        
        return this;
    }
}